//****************************************************************************
// lineTracker.c -
//****************************************************************************

#include "inc/hw_types.h"
#include "drivers/motor.h"

#include "drive.h"
#include "helper.h"
#include "lineIR.h"
#include "wireless.h"


//****************************************************************************
//
// Define the possible states for the EvalBot autonomous state machine
//
//****************************************************************************
#define GO_SPEED   60
#define STOP_SPEED   0


DanceInstruction previousInstruction;

DanceInstruction rotateRight()
{
	DanceInstruction instruction;

	instruction.leftDirection = FORWARD;
	instruction.leftWheelSpeed = GO_SPEED;
	instruction.rightDirection = REVERSE;
	instruction.rightWheelSpeed = GO_SPEED;

	return instruction;
}

DanceInstruction rotateLeft()
{
	DanceInstruction instruction;

	instruction.leftDirection = REVERSE;
	instruction.leftWheelSpeed = GO_SPEED;
	instruction.rightDirection = FORWARD;
	instruction.rightWheelSpeed = GO_SPEED;

	return instruction;
}

DanceInstruction turnRight()
{
	DanceInstruction instruction;

	instruction.leftDirection = FORWARD;
	instruction.leftWheelSpeed = GO_SPEED;
	instruction.rightDirection = FORWARD;
	instruction.rightWheelSpeed = STOP_SPEED;

	return instruction;
}

DanceInstruction turnLeft()
{
	DanceInstruction instruction;

	instruction.leftDirection = FORWARD;
	instruction.leftWheelSpeed = STOP_SPEED;
	instruction.rightDirection = FORWARD;
	instruction.rightWheelSpeed = GO_SPEED;

	return instruction;
}

DanceInstruction goStraight()
{
	DanceInstruction instruction;

	instruction.leftDirection = FORWARD;
	instruction.leftWheelSpeed = GO_SPEED;
	instruction.rightDirection = FORWARD;
	instruction.rightWheelSpeed = GO_SPEED;

	return instruction;
}

DanceInstruction stop()
{
	DanceInstruction instruction;

	instruction.leftDirection = FORWARD;
	instruction.leftWheelSpeed = STOP_SPEED;
	instruction.rightDirection = FORWARD;
	instruction.rightWheelSpeed = STOP_SPEED;

	return instruction;
}

void TrackLine()
{
	LineReading reading = GetLineReading();
	DanceInstruction nextInstruction;

	displayLineReading("Follow Line", reading);

	switch (reading)
	{
	case LEFT_READ:
		nextInstruction = rotateLeft();
		break;

	case MIDDLELEFT_READ:
		nextInstruction = turnLeft();
		break;

	case MIDDLE_READ:
		nextInstruction = goStraight();
		break;

	case MIDDLERIGHT_READ:
		nextInstruction = turnRight();
		break;

	case RIGHT_READ:
		nextInstruction = rotateRight();
		break;

	case ALL_READ:
		nextInstruction = previousInstruction; // All sensing the line so just turn right.
		break;

	case SPLIT_READ:
		nextInstruction = previousInstruction; // unknown; so just continue
		break;

	case NONE_READ:
		nextInstruction = previousInstruction; // No line so just stop
		break;

	default:
		nextInstruction = stop(); // unknown; just continue
		break;
	}


	SendInstruction(nextInstruction);
	DriveInstruction(nextInstruction);
	previousInstruction = nextInstruction;
}

void LineTrackerInit()
{
	previousInstruction = goStraight();
}
